import numpy as np
from scipy.spatial.transform import Rotation as R

v = np.array([3, 5, 0])
axis = np.array([4, 4, 1])
theta = 1.2  # radian


# 旋转都是沿着坐标轴逆时针
def by_rotvec():
    axis2 = axis / np.linalg.norm(axis)
    print(1111, np.linalg.norm(axis2), axis2)

    rotation_vector = theta * axis2
    rotation = R.from_rotvec(rotation_vector)
    rotated_vec = rotation.apply(v)
    print(333, rotated_vec)

def by_quat():
    axis2 = axis / np.linalg.norm(axis)
    print(1111, np.linalg.norm(axis2), axis2)
    axis3 = axis2 * np.sin(1.2/2)
    quat = R.from_quat([*axis3, np.cos(1.2 / 2)])
    print(444, quat.as_rotvec())
    print(555, quat.apply(v))
by_rotvec()
by_quat()
